﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Bluefly
{


    public class KalmanFilteredVario : IDatasource
    {


        public enum VarioType
        {

            KALMAN_VARIO = 1,
            DAMPED_VARIO = 2
        }

        private double varDamp;


        public double var;
        public double dampedVar;

        private VarioType varType;

        public double lastVar;


        public double maxVarSinceLast = -1000.0;
        public double minVarSinceLast = 1000.0;
        public double avgVarSinceLast = 0.0;
        public int countSinceLast = 0;

        public KalmanFilteredAltitude altitude;


        public KalmanFilteredVario(KalmanFilteredAltitude altitude, double varDamp, VarioType varType)
        {
            this.altitude = altitude;
            this.varDamp = varDamp;
            this.varType = varType;
        }

        public void setVarDamp(double varDamp)
        {
            this.varDamp = varDamp;
        }



        public double Value
        {
            get
            {
                if (varType == VarioType.DAMPED_VARIO)
                {
                    return dampedVar;
                }
                return var;
            }
        }



        public double[] getMinMaxAvgSinceLast()
        {
            double[] ret = null;
            if (countSinceLast > 0)
            {
                ret = new double[] { minVarSinceLast, maxVarSinceLast, avgVarSinceLast, countSinceLast };
            }
            else
            {
                ret = new double[] { 0.0, 0.0, 0.0, 0.0 };
            }
            maxVarSinceLast = -1000.0;
            minVarSinceLast = 1000.0;
            avgVarSinceLast = 0.0;
            countSinceLast = 0;
            return ret;
        }


        public double addData(double time)
        {


            var = altitude.getVarioValue();
            dampedVar = dampedVar + varDamp * (var - dampedVar);

            if (varType == VarioType.DAMPED_VARIO)
            {

                lastVar = dampedVar;


                if (dampedVar > maxVarSinceLast)
                {
                    maxVarSinceLast = dampedVar;
                }
                if (dampedVar < minVarSinceLast)
                {
                    minVarSinceLast = dampedVar;
                }
                countSinceLast++;
                avgVarSinceLast += (dampedVar - avgVarSinceLast) / countSinceLast;
                return dampedVar;

            }
            else
            {

                lastVar = var;


                if (var > maxVarSinceLast)
                {
                    maxVarSinceLast = var;
                }
                if (var < minVarSinceLast)
                {
                    minVarSinceLast = var;
                }
                countSinceLast++;
                avgVarSinceLast += (var - avgVarSinceLast) / countSinceLast;
                return var;

            }


        }


    }
}
